﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
//v0.1
namespace lighttool.NavMesh
{
    public class FindNode
    {
        public int nodeid;
        public int pathSessionId;
        public int ParentID = -1;
        public bool Open;
        public double HValue;   //H评估值
        public double GValue;   //G评估值
        public int ArrivalWall;
        /// <summary>
        /// 计算三角形估价函数（h值）
        /// 使用该三角形的中心点（3个顶点的平均值）到路径终点的x和y方向的距离。
        /// </summary>
        /// <param name="endPos">终点</param>
        public void CalcHeuristic(navMeshInfo info, navVec3 endPos)
        {
            var c = info.nodes[this.nodeid].center;
            double xDelta = Math.Abs(c.x - endPos.x);
            double zDelta = Math.Abs(c.z - endPos.z);
            HValue = Math.Sqrt(xDelta * xDelta + zDelta * zDelta);
        }
        public double GetCost(navMeshInfo info, int neighborID)
        {
            var bc = info.nodes[neighborID].center;
            var nc = info.nodes[this.nodeid].center;

            var xd = bc.x - nc.x;
            var yd = bc.y - nc.y;
            var zd = bc.z - nc.z;
            var d = Math.Sqrt(xd * xd + yd * yd + zd * zd);
            return d;
        }
    }
    public class pathFinding
    {

        public static int[] calcAStarPolyPath(navMeshInfo info, int startPoly, int endPoly, navVec3 endPos = null, float offset = 0.1f)
        {

            List<FindNode> nodeFind = new List<FindNode>();
            foreach (var node in info.nodes)
            {
                var nn = new FindNode();
                nn.nodeid = node.nodeID;
                nodeFind.Add(nn);
            }


            if (endPos == null)
            {
                endPos = info.nodes[endPoly].center.clone();
            }
            var startTri = nodeFind[startPoly];
            startTri.nodeid = startPoly;
            //////////////////////////////////// A*算法 ///////////////////////////////////////

            int pathSessionId = 1;
            bool foundPath = false;
            List<int> openList = new List<int>();   //开放列表
            List<int> closeList = new List<int>();

            startTri.pathSessionId = pathSessionId;

            openList.Add(startPoly);
            while (openList.Count > 0)
            {
                // 1. 把当前节点从开放列表删除, 加入到封闭列表
                FindNode currNode;
                currNode = nodeFind[openList[openList.Count - 1]];
                openList.Remove(currNode.nodeid);
                closeList.Add(currNode.nodeid);

                //已经找到目的地
                if (currNode.nodeid == endPoly)
                {
                    foundPath = true;
                    break;
                }

                // 2. 对当前节点相邻的每一个节点依次执行以下步骤:
                // 遍历所有邻接三角型
                var nlink = info.nodes[currNode.nodeid].getLinked(info);
                for (int i = 0; i < nlink.Length; i++)
                {
                    int neighborID = nlink[i];
                    FindNode neighborTri;

                    // 3. 如果该相邻节点不可通行,则什么操作也不执行,继续检验下一个节点;
                    if (neighborID < 0)
                    {
                        //没有该邻居节点
                        continue;
                    }
                    else
                    {
                        neighborTri = nodeFind[neighborID];

                        if (neighborTri == null || neighborTri.nodeid != neighborID)
                            return null;
                    }
                    //if (neighborTri.GetGroupID() == startTri.GetGroupID())
                    {
                        if (neighborTri.pathSessionId != pathSessionId)
                        {
                            //judge the side is wide enough to to pass in offset
                            var b = info.nodes[neighborTri.nodeid].isLinkTo(info, currNode.nodeid);

                            //int sideIndex = neighborTri.GetNeighborWall(info, currNode.nodeID);
                            // if (sideIndex != -1 && neighborTri.GetSide(sideIndex).GetLength() >= offset)
                            if (b != null && info.borders[b].length >= offset * 2)
                            //
                            {
                                // 4. 如果该相邻节点不在开放列表中,则将该节点添加到开放列表中, 
                                //    并将该相邻节点的父节点设为当前节点,同时保存该相邻节点的G和F值;
                                neighborTri.pathSessionId = pathSessionId;
                                neighborTri.ParentID = currNode.nodeid;
                                neighborTri.Open = true;

                                // 计算启发值h
                                neighborTri.CalcHeuristic(info, endPos);
                                // 计算三角形花费g
                                neighborTri.GValue = currNode.GValue + currNode.GetCost(info, neighborTri.nodeid);

                                //放入开放列表并排序
                                openList.Add(neighborTri.nodeid);
                                openList.Sort((x, y) =>
                                {
                                    var xFvalue = nodeFind[x].HValue + nodeFind[x].GValue;
                                    var yFvalue = nodeFind[y].HValue + nodeFind[y].GValue;

                                    if (xFvalue < yFvalue - 0.001)
                                        return 1;
                                    if (xFvalue > yFvalue + 0.001)
                                        return -1;
                                    return 0;
                                });

                                //保存穿入边
                                neighborTri.ArrivalWall = currNode.nodeid;
                            }
                        }
                        else
                        {
                            // 5. 如果该相邻节点在开放列表中, 
                            //    则判断若经由当前节点到达该相邻节点的G值是否小于原来保存的G值,
                            //    若小于,则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的G和F值
                            if (neighborTri.Open)
                            {
                                if (neighborTri.GValue + neighborTri.GetCost(info, currNode.nodeid) < currNode.GValue)
                                {
                                    currNode.GValue = neighborTri.GValue + neighborTri.GetCost(info, currNode.nodeid);
                                    currNode.ParentID = neighborTri.nodeid;
                                    currNode.ArrivalWall = neighborTri.nodeid;
                                }
                            }
                            else
                            {
                                neighborTri = null;
                                continue;
                            }

                        }
                    }
                }
            }

            List<int> outpath = new List<int>();
            if (closeList.Count != 0)
            {
                FindNode path = nodeFind[closeList[closeList.Count - 1]];
                outpath.Add(path.nodeid);

                while (path.ParentID != -1)
                {
                    outpath.Add(path.ParentID);
                    path = nodeFind[path.ParentID];
                }
            }

            //if (closeList.Count != 0)
            //{
            //    NavTriangle path = closeList[closeList.Count - 1];
            //    pathList.Add(path);
            //    while (path.GetParentID() != -1)
            //    {
            //        pathList.Add(this.m_lstTriangle[path.GetParentID()]);
            //        path = this.m_lstTriangle[path.GetParentID()];
            //    }
            //}

            if (!foundPath)
                return null;
            else
                return outpath.ToArray();
        }

        static double NearAngle(double a, double b)
        {
            double aa = a;
            if (a >= 180)
                aa = 360 - a;
            double bb = b;
            if (b >= 180)
                bb = 360 - b;
            if (aa < bb)
                return a;
            else
                return b;
        }
        public static navVec3[] FindPath(navMeshInfo info, navVec3 startPos, navVec3 endPos, float offset = 0.1f)
        {
            int startPoly = -1;
            int endPoly = -1;
            for (int p = 0; p < info.nodes.Length; p++)
            {
                if (info.inPoly(startPos, info.nodes[p].poly))
                {
                    startPoly = p;
                }
                if (info.inPoly(startPos, info.nodes[p].poly))
                {
                    endPoly = p;
                }
            }
            int[] polypath = calcAStarPolyPath(info, startPoly, endPoly, endPos, offset);
            return calcWayPoints(info, startPos, endPos, polypath, offset);
        }
        public static navVec3[] calcWayPoints(navMeshInfo info, navVec3 startPos, navVec3 endPos, int[] polyPath, float offset = 0.1f)
        {
            var wayPoints = new List<navVec3>();
            if (polyPath.Length == 0 || startPos == null || endPos == null)
                return null;

            var triPathList = new List<int>(polyPath);
            // 保证从起点到终点的顺序
            triPathList.Reverse();

            wayPoints.Add(startPos);


            int ipoly = 0;//从第0个poly 开始检查
            navVec3 dirLeft = null;
            int ipolyLeft = -1;
            navVec3 dirRight = null;
            int ipolyRight = -1;
            int breakdir = 0;
            navVec3 posLeft = null;
            navVec3 posRight = null;
            navVec3 posNow = startPos.clone();

            for (int c = 0; c < 100; c++)//最多循环100次
            {
                for (int i = ipoly; i < triPathList.Count; i++)
                {
                    if (i == triPathList.Count - 1)//最后一节
                    {
                        if (dirLeft == null || dirRight == null)
                        {
                            breakdir = 0;
                            break;
                        }
                        else
                        {
                            navVec3 dirFinal = navVec3.NormalAZ(posNow, endPos);

                            var a1 = navVec3.Angle(dirLeft, dirFinal);
                            var b1 = navVec3.Angle(dirRight, dirFinal);
                            if (a1 * b1 > 0)
                            {
                                if (a1 > 0)
                                    breakdir = 1;
                                else
                                    breakdir = -1;
                                //System.Diagnostics.Debug.WriteLine("不能直接到达终点");
                            }
                            else
                            {
                                breakdir = 0;
                                break;
                            }
                        }
                    }
                    else//检查是否通过
                    {
                        //寻找边
                        var n1 = triPathList[i];
                        var n2 = triPathList[i + 1];
                        string bname = n1 + "-" + n2;
                        if (n2 < n1)
                            bname = n2 + "-" + n1;
                        var border = info.borders[bname];
                        var pointA = navVec3.Border(info.vecs[border.pointA], info.vecs[border.pointB], offset);
                        var pointB = navVec3.Border(info.vecs[border.pointB], info.vecs[border.pointA], offset);

                        var dist1 = navVec3.DistAZ(posNow, pointA);
                        var dist2 = navVec3.DistAZ(posNow, pointB);
                        if (dist1 < 0.001 || dist2 < 0.001)
                            continue;
                        if (dirLeft == null)
                        {
                            dirLeft = navVec3.NormalAZ(posNow, pointA);
                            posLeft = pointA.clone();
                            ipolyLeft = i;
                        }
                        if (dirRight == null)
                        {
                            dirRight = navVec3.NormalAZ(posNow, pointB);
                            posRight = pointB.clone();
                            ipolyRight = i;
                        }
                        var adir = navVec3.Angle(dirLeft, dirRight);
                        if (adir < 0)//change
                        {
                            var p = dirLeft;
                            var pp = posLeft;
                            var pi = ipolyLeft;
                            dirLeft = dirRight;
                            posLeft = posRight;
                            ipolyLeft = ipolyRight;
                            dirRight = p;
                            posRight = pp;
                            ipolyRight = pi;
                        }

                        if (ipolyLeft != i || ipolyRight != i)//检查是否能穿越
                        {
                            var ndirLeft = navVec3.NormalAZ(posNow, pointA);
                            var ndirRight = navVec3.NormalAZ(posNow, pointB);
                            var nadir = navVec3.Angle(ndirLeft, ndirRight);
                            if (nadir < 0)//change
                            {
                                var p = ndirLeft;
                                var pp = pointA;
                                ndirLeft = ndirRight;
                                pointA = pointB;
                                ndirRight = p;
                                pointB = pp;
                            }

                            var aLL = navVec3.Angle(dirLeft, ndirLeft);//>0 右侧，<0 左侧
                            var aRL = navVec3.Angle(dirRight, ndirLeft);//>0 右侧，<0 左侧
                            var aLR = navVec3.Angle(dirLeft, ndirRight);//>0 右侧，<0 左侧
                            var aRR = navVec3.Angle(dirRight, ndirRight);//>0 右侧，<0 左侧
                            if ((aLL < 0 && aLR < 0))//无法穿越
                            {
                                breakdir = -1;
                                break;
                            }
                            if (aRL > 0 && aRR > 0)//无法穿越
                            {
                                breakdir = 1;
                                break;
                            }
                            if (aLL > 0 && aRL < 0)
                            {
                                dirLeft = ndirLeft;
                                posLeft = pointA;
                                ipolyLeft = i;
                            }
                            if (aLR > 0 && aRR < 0)
                            {
                                dirRight = ndirRight;
                                posRight = pointB;
                                ipolyRight = i;
                            }
                        }

                    }
                }

                if (breakdir == 0)
                {
                    break;
                }
                else
                {
                    if (breakdir == -1)
                    {
                        wayPoints.Add(posLeft.clone());
                        posNow = posLeft;
                        ipoly = ipolyLeft;
                    }
                    else
                    {
                        wayPoints.Add(posRight.clone());
                        posNow = posRight;
                        ipoly = ipolyRight;
                    }
                    dirLeft = null;
                    dirRight = null;
                    ipolyLeft = -1;
                    ipolyRight = -1;
                }

            }




            wayPoints.Add(endPos);

            return wayPoints.ToArray();
        }

    }
}
